#include "SlerpInterpolator.h"
#include <math.h>

SlerpInterpolator::SlerpInterpolator(const std::vector<Snapshot>& snapshots) : m_nodes(snapshots.size())
{
	m_nodes.clear();
	std::vector<Snapshot>::const_iterator snapShotIterator;
	for(snapShotIterator = snapshots.begin(); snapShotIterator != snapshots.end(); ++snapShotIterator)
	{
		SquadNode node;
		node.rotation = snapShotIterator->rotation;
		m_nodes.push_back(node);
	}
}

SlerpInterpolator::~SlerpInterpolator(void)
{
	m_nodes.clear();
}

DirectX::XMVECTOR SlerpInterpolator::GetSlerpedRotation( double time )
{
	using namespace DirectX;
	if(m_nodes.size() == 0)
	{
		return XMQuaternionIdentity();
	}
	assert(time >= 0.);
	int n1 = static_cast<int>(floor(time)) % m_nodes.size();
	int n2 = (n1 + 1) % m_nodes.size();
	XMFLOAT4 qh = m_nodes[n1].rotation;
	XMFLOAT4 rh = m_nodes[n2].rotation;
	XMVECTOR q, r, slerped;
	q = XMLoadFloat4(&qh);
	r = XMLoadFloat4(&rh);
	double t = fmod(time, 1.);
	float sigma = asin(qh.x*rh.x + qh.y*rh.y + qh.z*rh.z + qh.w*rh.w);
	return XMQuaternionNormalize((sin(sigma*(1-t)))/sin(sigma)*q + (sin(sigma*t))/sin(sigma)*r);
}